/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "owndebug.h"
#include "ecan1000hr.hpp"

#include "ownutils.h"


#include <errno.h>

#ifndef LINUX_OS
 #warning ECAN1000HR implementation removed
#endif

#ifdef LINUX_OS
BitRates ecan1000drv_rates[] = {
  R50000,
  R62500,
  R100000,
  R125000,
  R250000,
  R500000,
  R1000000
};
#endif

//*****************************************************************************
//*****************************************************************************

CEcan1000hr::CEcan1000hr() :
  iRxQueue(0),
  iTxQueue(0),
  iBitRate(0),
  iDeviceHandle(0),
  iExtended(false),
  iRemote(false),
  iID(),
  iNextMsg(false),
  iDontQueueUse(false),
  iID_ACR(0),
  iID_AMR(0),
  iRTR_ACR(0),
  iRTR_AMR(0),
  iData_ACR(0),
  iData_AMR(0)
{
  dPrint(8,"Ecan1000hr default constructor called\n");

}

CEcan1000hr::CEcan1000hr(int aRxQueue,
                        int aTxQueue,int aBitRate)
  : iRxQueue(aRxQueue),
    iTxQueue(aTxQueue),
    iBitRate(aBitRate),
    iDeviceHandle(0),
    iExtended(false),
    iRemote(false),
    iID(),
    iNextMsg(false),
    iDontQueueUse(false),
    iID_ACR(0),
    iID_AMR(0),
    iRTR_ACR(0),
    iRTR_AMR(0),
    iData_ACR(0),
    iData_AMR(0)
{
  dPrint(8,"Ecan1000hr advanced constructor called\n");

}

CEcan1000hr::~CEcan1000hr()
{
  Close();
}


CASRoboBus::EBusStatus CEcan1000hr::Open(void){
  return KBusStatusOK;
}


CASRoboBus::EBusStatus CEcan1000hr::Init(void)
{

#ifdef LINUX_OS



  int boardNumber=0;

  //Open the ECAN interface
  //iDeviceHandle = Ecan_CreateHandle(boardNumber, ECAN1000DRV_BOARD_TYPE);
  Lock();
  iDeviceHandle = Ecan_CreateHandle(boardNumber, 1000);

  Unlock();
  if (iDeviceHandle < 0) {
    dPrint(3,"Ecan_CreateHandle() FAILED,iDeviceHandle %d",iDeviceHandle);
    return KBusStatusError;
  }

  /*
   * Set the bitrate of the bus
  */

  if (! Ecan_SetBitRate(iDeviceHandle, ecan1000drv_rates[iBitRate] )) {
    dPrint(3,"Ecan_SetBitRate() FAILED");
    return KBusStatusError;
  }



  /*
   * Put the board into operating mode
   */
   if (! Ecan_StartBoard(iDeviceHandle)) {

    dPrint(3,"Ecan_StartBoard() FAILED");
    return KBusStatusError;

   }


   // Set receive queue size
  rtd_ecan_set_rx_max_queue_size(iDeviceHandle, iRxQueue);


  //Set transmit queue size
  rtd_ecan_set_tx_max_queue_size(iDeviceHandle, iTxQueue);

  /*
  ////////////////////////////////////////////////////////
  //Fixme: For some reason without this, the can driver
  //wont work every second time.... weird(!)
  /// NO NEED FOR THIS.... IT WAS BECAUSE THE DRIVER DON'T SUPPORT
  /// TO HAVE COMMAND FROM MULTIPLE THREADS!!
  SetStructureParameters(false, false,0x1C0,false ,false);
  TGIMI_PROTOCOL_AVANT_CMD_V3 iHeader;
  iHeader.SetIndex(0);	
  iHeader.SetThrottle(0); iHeader.SetBreaks(true);
  iHeader.SetGear(TGIMI_PROTOCOL_AVANT_CMD_V3::gearFree);
  iHeader.Print(1);
  int s =   Write((unsigned char *)(&iHeader), sizeof(iHeader));

  dPrint(1,"s = %d",s);

  TGIMI_PROTOCOL_AVANT_REPLY_V3 reply;	
  Read((unsigned char *)&iHeader,sizeof(iHeader));
  reply.Print(1);

  ///////////////////////////////////////////////////////
  */

  dPrint(3,"Ecan initialized!");
#endif
  return KBusStatusOK;
}
//*****************************************************************************

CASRoboBus::EBusStatus CEcan1000hr::Close(void)
{
#ifdef LINUX_OS
  Lock();
  if (! Ecan_StopBoard(iDeviceHandle)) {
    dPrint(3,"Ecan_StopBoard() FAILED");
    Unlock();
    return KBusStatusError;
  }
  Unlock();
#endif
  return KBusStatusOK;
}
//*****************************************************************************

int CEcan1000hr::Read(unsigned char *buffer, int buffer_size)
{
#ifdef LINUX_OS
    int return_value = 0,i=0,i2=0;
    ulong messages_waiting = 0;
    uint event_mask= 0;
    TEcanMessage message;

    /*
     * Get the number of messages waiting in the receive queue
     */
    Lock();
    if (! Ecan_GetQueuesCounts(iDeviceHandle, NULL, &messages_waiting)) {
      dPrint(6,"Ecan_GetQueuesCounts() FAILED\n");
      Unlock();
      return -1;
    }
    Unlock();

    // dPrint(10,"MEssages waiting %u, size = %u, buffer_size = %d",
    //  messages_waiting,messages_waiting*sizeof(TEcanMessage),buffer_size);
    /*
     * See if there are messages waiting in receive queue
     */

    if (messages_waiting == 0){
      return 0;
    }


    /*
     * Prepare the first waiting message to be received
     */
    Lock();
    event_mask = Ecan_GetInterrupts(iDeviceHandle);
    Unlock();
    if (event_mask == 0) {
      dPrint(6,"Ecan_GetInterrupts() FAILED\n");
      return -1;
    }

    if (event_mask == 0xFF) {
      dPrint(6,"Ecan_GetInterrupts() FAILED: Receive queue overflow\n");
      return -1;
    }

    /*
     * Determine what type of event this message is
     */
    // dPrint(10,"buffer_size = %d",buffer_size);
    if (event_mask & RTD_ECAN_EVENT_RECEIVE) {
      //ECAN_MESSAGE_STRUCTURE	message;
      //ssize_t			bytes_output;
      //uint8_t			data_byte;

      /*
       * This message was received from transmitter
       */

      if(buffer_size <=8){
	dPrint(10,"buffer_size = %d",buffer_size);
	Lock();
	if (! Ecan_GetMessage(iDeviceHandle, &message)) {
	  dPrint(3,"Ecan_GetMessage() FAILED\n");
	  Unlock();
	  return -1;

	}
	Unlock();
	for(i = 0;i < buffer_size;i++){
	  buffer[i] = message.Data[i];
	  ++return_value;
	}
      }else{
       dPrint(10,"buffer_size = %d",buffer_size);
	for(i= 0;i<(int)(buffer_size/8+0.5);i++){
	  Lock();
	  if (! Ecan_GetMessage(iDeviceHandle, &message)) {
	    dPrint(3,"Ecan_GetMessage() FAILED\n");
	    Unlock();
	    return -1;
	  }
	  Unlock();
	  for(i2=0;i2<8;i2++){
	    buffer[i2+i] = message.Data[i2];
	    ++return_value;
	  }
	}
      }



    }


    /*
     * Take care of any error status
     */

    if (event_mask & RTD_ECAN_EVENT_ERROR_WARN_LIMIT) {
      dPrint(6,"Ecan_GetInterrupts() FAILED: Error warning limit exceeded\n");
      return -1;
    }

    if (event_mask & RTD_ECAN_EVENT_DATA_OVERRUN) {
      dPrint(6,"Ecan_GetInterrupts() FAILED: Data overrun\n");
      return -1;
    }

    if (event_mask & RTD_ECAN_EVENT_ERROR_PASSIVE) {
      dPrint(6,	"Ecan_GetInterrupts() FAILED: Bus in Error Passive state\n");
      return -1;
    }

    if (event_mask & RTD_ECAN_EVENT_ARBITRATION_LOST) {
      dPrint(6,"Ecan_GetInterrupts() FAILED: Arbitration lost\n");
      return -1;
    }

    if (event_mask & RTD_ECAN_EVENT_BUS_ERROR) {
      dPrint(6,"Ecan_GetInterrupts() FAILED: Bus error\n");
      return -1;
    }

    return return_value;
#else
    return 0;
#endif
}
//*****************************************************************************

int CEcan1000hr::Write(const unsigned char *buffer, int buffer_size)
{
#ifdef LINUX_OS
  static int errorsCount = 0;
  //dPrint(1,"Write called at can,size %d,dontqueue %d",buffer_size, iDontQueueUse);
  int writed = 0,i = 0,i2 = 0;
  dPrint(10,"%d %d %d %d %d %d %d %d",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4],buffer[5], buffer[6],buffer[7]);

  //Constructing the structure
  Lock();
  TEcanMessage message;
  message.Channel = 0;

  message.Extended = iExtended;
  message.SetID(iID);
  message.NextMsg = iNextMsg;
  message.Remote = iRemote;
 
  message.DontQueueUse = iDontQueueUse;

  Unlock();



  ECAN_STATUS_STRUCTURE	status;


  struct timespec sleep_time;

  for(i = 0; i <= (buffer_size-1)/8; i++){

    for(i2 = 0; i2< 8; i2++){
      message.Data[i2] = (buffer[(i*8)+i2]);
    }



    if(buffer_size -(i*8) >= 8){
       message.DataLength = 8;
    }else{
      message.DataLength = buffer_size -(i*8);
    }



    /*
     * Attempt to transmit the message
     */
    Lock();
    if (! Ecan_SendMessage(iDeviceHandle, &message)){
      errorsCount++;
      int errsv = errno;
      dPrint(3,"Ecan_SendMessage() FAILED\n");

      if(errsv ==EBADF){
	;
	dPrint(5,"handle is not a valid file descriptor.  ");
      }else if(errsv ==EBUSY){

	dPrint(5,"The Transmission Buffer Status bit in the Control Register is cleared and dont_use_queue is nonzero (ECAN1000 only)");
      }else if(errsv ==EFAULT ){

	dPrint(5,"message_p if not a valid user address");
      }else if(errsv == ENOBUFS){

	dPrint(5,"dont_use_queue is 0 and there is no free slot in the driver's transmit queue");
      }else if(errsv == EPERM){
	dPrint(5,"The Reset Mode bit is set in the Mode Register and dont_use_queue is nonzero (ECAN1000 only).");

      }
      Unlock();
      return -1;
    }
    Unlock();
    writed += message.DataLength;

   
    /*
     * Sleep for a bit to allow the board to update its status
     */
    sleep_time.tv_sec = 0;
    sleep_time.tv_nsec = 50000000; ///< WAS 50ms

    if (nanosleep(&sleep_time, NULL) == -1) {
      dPrint(8,"nanosleep() FAILED\n");
    }

    /*
     * Get board status
     */

    Lock();
    if (! Ecan_GetStatus(iDeviceHandle, &status)) {
      dPrint(5,"Ecan_GetStatus() FAILED\n");
      Unlock();
      return -1;
    }
    Unlock();

    /*
     * If no error occurred, no need to retransmit
     */

    if (status.ErrorCode != 0) {
      //An error occurred
      dPrint(5,"Board returned status of 0x%x\n",status.ErrorCode);
      return -1;
    }

  }

 return writed;
#else
return 0;
#endif
}
//*****************************************************************************

int CEcan1000hr::ReadBlock(unsigned char *buffer, int block_size, int timeout_ms)
{
  //dPrint(1,"Called ReadBlock, with block_size %d, timeout %d",block_size, timeout_ms);
  int r;
  unsigned int readed=0;
  bool cont = true;
  if (timeout_ms < 0) return -1;
  unsigned int timeBegin = ownTime_get_ms();
  unsigned char *message = (unsigned char *)malloc(block_size);
  while(cont) {  // REDO until the full timeout has elapsed OR packet is COMPLETE.
    if(ownTime_get_ms_since(timeBegin) >= timeout_ms){
      if(timeout_ms != 0){
	dPrint(10,"timesince(%d) >= timeout(%d)",ownTime_get_ms_since(timeBegin),timeout_ms);
      }
      free(message);
      return 0;
    }

    r = Read(message,block_size);

    if(r>0){
      //dPrint(1,"readed  + r = %u",readed+(unsigned int)r);
      if((unsigned int)block_size > readed+(unsigned int)r){
	buffer += readed;
	memcpy(buffer,message,r);
	buffer -=readed;
	readed +=(unsigned int)r;
	
      }else{
	
	buffer += readed;
	memcpy(buffer,message,r);
	readed +=(unsigned int)r;
	cont = false;
      }
    
    }else{
   
      struct timespec sleep_time;
      sleep_time.tv_sec = 0;
      sleep_time.tv_nsec = 2000000; ///< WAS 2ms
      
      if (nanosleep(&sleep_time, NULL) == -1) {
	dPrint(8,"nanosleep() FAILED\n");
      }
      
    }

    
  }
  free(message);
  

  return readed;

}
//*****************************************************************************

int CEcan1000hr::WriteBlock(unsigned char *buffer, int block_size, int timeout_ms)
{
  int writed=0;
  int r;
  bool cont = true;
  if (timeout_ms < 0) return -1;
  unsigned int timeBegin = ownTime_get_ms();

  while(cont) {  // REDO until the full timeout has elapsed OR packet is COMPLETE.
    if(ownTime_get_ms_since(timeBegin) >= timeout_ms){
      dPrint(10,"timesince(%d) >= timeout(%d)",ownTime_get_ms_since(timeBegin),timeout_ms);
      cont = false;

    }

    r= Write(buffer+writed,block_size-writed);
    if(r> 0){
      writed +=r;
    }
    if(writed ==block_size){
      cont = false;
    }

  }

  return writed;


}
//*****************************************************************************

bool CEcan1000hr::SetStructureParameters(bool aExtended,bool aRemote,u32_t aID , bool aNextMsg, bool aDontQueueUse)
{
dPrint(3,"SetstructureParameters called, dontusequeue %d!", aDontQueueUse);
  Lock();
  iExtended =aExtended;
  iRemote =aRemote;
  iID = aID;
  iNextMsg=aNextMsg;
  iDontQueueUse=aDontQueueUse;
  Unlock();
  return true;
}

//*****************************************************************************
bool CEcan1000hr::SetSingleFilter(unsigned int aID_ACR,unsigned int aID_AMR,
				  unsigned int aRTR_ACR,unsigned int aRTR_AMR,
				  unsigned int aData_ACR, unsigned int aData_AMR)
{
  bool result = false;
#ifdef LINUX_OS
  int i = 0;

  Lock();
  iID_ACR = aID_ACR;
  iID_AMR = aID_AMR;
  iRTR_ACR = aRTR_ACR;
  iRTR_AMR = aRTR_AMR;
  iData_ACR = aData_ACR ;
  iData_AMR = aData_AMR;

  while(!result && i <20){
    result = (Ecan_SetSingleFilterStandard(iDeviceHandle,aID_ACR,aID_AMR,
					   aRTR_ACR,aRTR_AMR, aData_ACR,
					   aData_AMR));
    i++;

  }

  /* if (! Ecan_StartBoard(iDeviceHandle)) {
    dPrint(3,"Ecan_StartBoard() FAILED");
    result = false;
    }*/
  Unlock();
#endif
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

